using System.Runtime.CompilerServices;
#if MYCODE
using BepuUtilities.Vectors;
#else
using System.Numerics;
#endif

namespace BepuUtilities
{
    /// <summary>
    /// 对称3x3矩阵的左下三角形(包括对角线)。
    /// </summary>
    public struct Symmetric3x3
    {
        /// <summary>
        /// 矩阵的第一行、第一列。
        /// </summary>
        public float XX;
        /// <summary>
        /// 矩阵的第二行,第一列。
        /// </summary>
        public float YX;
        /// <summary>
        /// 矩阵的第二行、第二列。
        /// </summary>
        public float YY;
        /// <summary>
        /// 矩阵的第三行,第一列。
        /// </summary>
        public float ZX;
        /// <summary>
        /// 矩阵的第三行第二列。
        /// </summary>
        public float ZY;
        /// <summary>
        /// 矩阵的第三行,第三列。
        /// </summary>
        public float ZZ;

        /// <summary>
        /// 计算对称矩阵m和旋转矩阵R的RT*m*r。
        /// </summary>
        /// <param name="r">用作三明治面包的旋转矩阵。</param>
        /// <param name="m">多肉内部对称矩阵。</param>
        /// 对称矩阵m的v*m*转置(V)的<param name="三明治">结果。</param>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void RotationSandwich(in Matrix3x3 r, in Symmetric3x3 m, out Symmetric3x3 sandwich)
        {
            // TODO：我们刚刚从广泛的实现中复制了这一点。如果有必要的话,有很多方法可以改善这一点。
            // (认真优化它几乎是不值得的-在撰写本文时,它只由姿势积分器调用,它是
            // 无论如何都是可怕的记忆束缚。)
            var i11 = r.X.X * m.XX + r.Y.X * m.YX + r.Z.X * m.ZX;
            var i12 = r.X.X * m.YX + r.Y.X * m.YY + r.Z.X * m.ZY;
            var i13 = r.X.X * m.ZX + r.Y.X * m.ZY + r.Z.X * m.ZZ;

            var i21 = r.X.Y * m.XX + r.Y.Y * m.YX + r.Z.Y * m.ZX;
            var i22 = r.X.Y * m.YX + r.Y.Y * m.YY + r.Z.Y * m.ZY;
            var i23 = r.X.Y * m.ZX + r.Y.Y * m.ZY + r.Z.Y * m.ZZ;

            var i31 = r.X.Z * m.XX + r.Y.Z * m.YX + r.Z.Z * m.ZX;
            var i32 = r.X.Z * m.YX + r.Y.Z * m.YY + r.Z.Z * m.ZY;
            var i33 = r.X.Z * m.ZX + r.Y.Z * m.ZY + r.Z.Z * m.ZZ;

            sandwich.XX = i11 * r.X.X + i12 * r.Y.X + i13 * r.Z.X;
            sandwich.YX = i21 * r.X.X + i22 * r.Y.X + i23 * r.Z.X;
            sandwich.YY = i21 * r.X.Y + i22 * r.Y.Y + i23 * r.Z.Y;
            sandwich.ZX = i31 * r.X.X + i32 * r.Y.X + i33 * r.Z.X;
            sandwich.ZY = i31 * r.X.Y + i32 * r.Y.Y + i33 * r.Z.Y;
            sandwich.ZZ = i31 * r.X.Z + i32 * r.Y.Z + i33 * r.Z.Z;
        }

        /// <summary>
        /// 计算对称矩阵的行列式。
        /// </summary>
        /// <param name="m">要解释为对称的矩阵。</param>
        /// <return>解释为对称的矩阵的行列式。</return>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static float Determinant(in Symmetric3x3 m)
        {
            var m11 = m.YY * m.ZZ - m.ZY * m.ZY;
            var m21 = m.ZY * m.ZX - m.ZZ * m.YX;
            var m31 = m.YX * m.ZY - m.ZX * m.YY;
            return m11 * m.XX + m21 * m.YX + m31 * m.ZX;
        }

        /// <summary>
        /// 反转给定矩阵。
        /// </summary>
        /// <param name="m">要反转的矩阵。</param>
        /// <param name="Inverse">逆矩阵。</param>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public unsafe static void Invert(in Symmetric3x3 m, out Symmetric3x3 inverse)
        {
            var m11 = m.YY * m.ZZ - m.ZY * m.ZY;
            var m21 = m.ZY * m.ZX - m.ZZ * m.YX;
            var m31 = m.YX * m.ZY - m.ZX * m.YY;
            var determinantInverse = 1f / (m11 * m.XX + m21 * m.YX + m31 * m.ZX);

            var m22 = m.ZZ * m.XX - m.ZX * m.ZX;
            var m32 = m.ZX * m.YX - m.XX * m.ZY;

            var m33 = m.XX * m.YY - m.YX * m.YX;

            inverse.XX = m11 * determinantInverse;
            inverse.YX = m21 * determinantInverse;
            inverse.ZX = m31 * determinantInverse;
            inverse.YY = m22 * determinantInverse;
            inverse.ZY = m32 * determinantInverse;
            inverse.ZZ = m33 * determinantInverse;
        }

        /// <summary>
        /// 将两个矩阵的分量相加。
        /// </summary>
        /// <param name="a">要添加的第一个矩阵。</param>
        /// <param name="b">要添加的第二个矩阵。</param>
        /// <param name="result">组件等于两个输入矩阵组件相加的矩阵。</param>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void Add(in Symmetric3x3 a, in Symmetric3x3 b, out Symmetric3x3 result)
        {
            result.XX = a.XX + b.XX;
            result.YX = a.YX + b.YX;
            result.YY = a.YY + b.YY;
            result.ZX = a.ZX + b.ZX;
            result.ZY = a.ZY + b.ZY;
            result.ZZ = a.ZZ + b.ZZ;
        }

        /// <summary>
        /// 从a中减去b的分量。
        /// </summary>
        /// <param name="a">要减去的矩阵。</param>
        /// <param name="b">要从第一个矩阵减去的矩阵..</param>
        /// <param name="result">包含减去组件的矩阵。</param>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void Subtract(in Symmetric3x3 a, in Symmetric3x3 b, out Symmetric3x3 result)
        {
            result.XX = a.XX - b.XX;
            result.YX = a.YX - b.YX;
            result.YY = a.YY - b.YY;
            result.ZX = a.ZX - b.ZX;
            result.ZY = a.ZY - b.ZY;
            result.ZZ = a.ZZ - b.ZZ;
        }

        /// <summary>
        /// 将两个矩阵的分量相加。
        /// </summary>
        /// <param name="a">要添加的第一个矩阵。</param>
        /// <param name="b">要添加的第二个矩阵。</param>
        /// <param name="result">组件等于两个输入矩阵组件相加的矩阵。</param>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void Add(in Matrix3x3 a, in Symmetric3x3 b, out Matrix3x3 result)
        {
            var bX = new Vector3(b.XX, b.YX, b.ZX);
            var bY = new Vector3(b.YX, b.YY, b.ZY);
            var bZ = new Vector3(b.ZX, b.ZY, b.ZZ);
            result.X = a.X + bX;
            result.Y = a.Y + bY;
            result.Z = a.Z + bZ;
        }

        /// <summary>
        /// 从一个矩阵中减去另一个矩阵的分量。
        /// </summary>
        /// <param name="a">要减去的矩阵。</param>
        /// <param name="b">要从第一个矩阵中减去的矩阵。</param>
        /// <param name="result">分量等于两个输入矩阵之差的矩阵。</param>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void Subtract(in Matrix3x3 a, in Symmetric3x3 b, out Matrix3x3 result)
        {
            var bX = new Vector3(b.XX, b.YX, b.ZX);
            var bY = new Vector3(b.YX, b.YY, b.ZY);
            var bZ = new Vector3(b.ZX, b.ZY, b.ZZ);
            result.X = a.X - bX;
            result.Y = a.Y - bY;
            result.Z = a.Z - bZ;
        }

        /// <summary>
        /// 将矩阵中的每个分量乘以给定的比例。
        /// </summary>
        /// <param name="m">要缩放的矩阵。</param>
        /// <param name="scale">缩放以应用于原始矩阵的每个组件。</param>
        /// <param name="Scaled">缩放结果。</param>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void Scale(in Symmetric3x3 m, float scale, out Symmetric3x3 scaled)
        {
            scaled.XX = m.XX * scale;
            scaled.YX = m.YX * scale;
            scaled.YY = m.YY * scale;
            scaled.ZX = m.ZX * scale;
            scaled.ZY = m.ZY * scale;
            scaled.ZZ = m.ZZ * scale;
        }

        /// <summary>
        /// 将两个矩阵相乘,就好像它们是对称的一样。
        /// </summary>
        /// <param name="a">要相乘的第一个矩阵。</param>
        /// <param name="b">要相乘的第二个矩阵。</param>
        /// <param name="result">乘积。</param>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void MultiplyWithoutOverlap(in Symmetric3x3 a, in Symmetric3x3 b, out Symmetric3x3 result)
        {
            var ayxbyx = a.YX * b.YX;
            var azxbzx = a.ZX * b.ZX;
            var azybzy = a.ZY * b.ZY;
            result.XX = a.XX * b.XX + ayxbyx + azxbzx;

            result.YX = a.YX * b.XX + a.YY * b.YX + a.ZY * b.ZX;
            result.YY = ayxbyx + a.YY * b.YY + azybzy;

            result.ZX = a.ZX * b.XX + a.ZY * b.YX + a.ZZ * b.ZX;
            result.ZY = a.ZX * b.YX + a.ZY * b.YY + a.ZZ * b.ZY;
            result.ZZ = azxbzx + azybzy + a.ZZ * b.ZZ;
        }


        /// <summary>
        /// 将两个矩阵相乘。
        /// </summary>
        /// <param name="a">要相乘的第一个矩阵。</param>
        /// <param name="b">要相乘的第二个矩阵。</param>
        /// <param name="result">乘积。</param>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void Multiply(in Matrix3x3 a, in Symmetric3x3 b, out Matrix3x3 result)
        {
            var bX = new Vector3(b.XX, b.YX, b.ZX);
            var bY = new Vector3(b.YX, b.YY, b.ZY);
            var bZ = new Vector3(b.ZX, b.ZY, b.ZZ);
            {
                var x = new Vector3(a.X.X);
                var y = new Vector3(a.X.Y);
                var z = new Vector3(a.X.Z);
                result.X = x * bX + y * bY + z * bZ;
            }

            {
                var x = new Vector3(a.Y.X);
                var y = new Vector3(a.Y.Y);
                var z = new Vector3(a.Y.Z);
                result.Y = x * bX + y * bY + z * bZ;
            }

            {
                var x = new Vector3(a.Z.X);
                var y = new Vector3(a.Z.Y);
                var z = new Vector3(a.Z.Z);
                result.Z = x * bX + y * bY + z * bZ;
            }
        }

        /// <summary>
        /// 通过对称矩阵变换向量。
        /// </summary>
        /// <param name="v">要转换的向量。</param>
        /// <param name="m">要解释为对称转换的矩阵。</param>
        /// <param name="result">按给定对称矩阵转换向量的结果。</param>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void TransformWithoutOverlap(in Vector3 v, in Symmetric3x3 m, out Vector3 result)
        {
            result.X = v.X * m.XX + v.Y * m.YX + v.Z * m.ZX;
            result.Y = v.X * m.YX + v.Y * m.YY + v.Z * m.ZY;
            result.Z = v.X * m.ZX + v.Y * m.ZY + v.Z * m.ZZ;
        }

        public override string ToString()
        {
            return $"x: {XX}, y: {YX}, {YY}, z: {ZX}, {ZY}, {ZZ}";
        }
    }
}
